#pragma once

// Copyright (c) 2010 Manuel Peinado Gallego <manuel.peinado@gmail.com>
// Distributed under the MIT License

// Quat.h : interface of the Quat class
//

#include "alg3d/Vec3.h"
#include <iosfwd>

namespace gx
{
    class Mat3;
    class AxisAngle;

    // --------------------------------------------------------------
    // "Quat" class
    // --------------------------------------------------------------
    // Unit quaternions provide a convenient mathematical notation 
    // for representing orientations and rotations of objects in 3D.
    // Compared to Euler angles they are simpler to compose and avoid
    // the problem of gimbal lock. Compared to rotation matrices 
    // they are more numerically stable and may be more efficient.
    // (See http://bit.ly/9Y7kd4)
    // --------------------------------------------------------------

    class Quat
    {
    public:

        // ----------------------------------------------------------
        // Member variables (public on purpose)
        // If "w + xi + yj + zk" is any quaternion, then "w" is 
        // called its scalar part and "xi + yj + xk" is called its 
        // vector part. 
        // Note: it might make sense to make these private as it's
        // the only way to guarantee that the quaternion is 
        // unit-length
        // ----------------------------------------------------------
        
        Real w, x, y, z;

        // ----------------------------------------------------------
        // Create an unitialized quaternion
        // ----------------------------------------------------------

        Quat();

        // ----------------------------------------------------------
        // Create an identity/zero quaternion. Example of use:
        //   gx::Quat q(gx::eye);
        // ----------------------------------------------------------

        explicit Quat(Eye);
        explicit Quat(Zero); 
        
        // ----------------------------------------------------------
        // Create a quaternion from its individual components
        // Should be unit but this is not enforced
        // ----------------------------------------------------------

        Quat(Real w, Real x, Real y, Real z);
        Quat(Real w, const Vec3 & xyz);

        // ----------------------------------------------------------
        // Access the vector part of a quaternion (for the scalar 
        // part simply do q.w)
        // ----------------------------------------------------------

        Vec3 get_vec_part() const;
        void set_vec_part(const Vec3 & v);

        // ----------------------------------------------------------
        // Convert this quaternion to axis-angle representation
        // (http://bit.ly/d8IkIU)
        // ----------------------------------------------------------

        AxisAngle to_axis_angle() const;
        
        // ----------------------------------------------------------
        // Copies the components of this quaternion to an array
        // Returns the array itself
        // ----------------------------------------------------------

        Real * to_array(Real dest[4]) const;

        // ----------------------------------------------------------
        // Compute shorter distance and regular distance between
        // two quaternions
        // ----------------------------------------------------------

        Real shorter_dist(const Quat& other) const;
        Real dist(const Quat& other) const;

        // ----------------------------------------------------------
        // Computes the dot product of two quaternions
        // ----------------------------------------------------------

        Real dot(const Quat& rhs) const;

        // ----------------------------------------------------------
        // Compute the squared norm and the norm of a quaternion
        // ----------------------------------------------------------

        // The square root of the product of a quaternion with its 
        // conjugate is called its norm
        Real norm2() const;
        Real norm() const;

        // ----------------------------------------------------------
        // Initializes a quaternion with an axis-angle rotation
        // (http://bit.ly/bhQYHT)
        // ----------------------------------------------------------

        Quat & from_axis_angle(AxisAngle const & aa);

        // ----------------------------------------------------------
        // Creates a quaternion that rotates by shortest arc from 
        // to vectors (http://bit.ly/d1SKTP). The vectors are
        // supposed to be unit-length
        // ----------------------------------------------------------

        Quat & from_two_vectors(Vec3 const & v, Vec3 const & w);

        // ----------------------------------------------------------
        // Compares two quaternions for equality
        // ----------------------------------------------------------

        bool equals(Quat const & q, Real tolerance = 1e-8) const;

        // ----------------------------------------------------------
        // Turns a quaternion into its own unit-length, conjugate
        // or reciprocal version. These three methods modify the
        // object they are called on. To obtain a modified copy and
        // leave the original object intact, use the copy ctor:
        //     Quat conj = Quat(q).conjugate();
        // If you find this ugly you can implement a free function 
        // that performs the copy:
        //     Quat conj(const Quat & src) { 
        //        return Quat(src).conjugate(); 
        //     }
        // ----------------------------------------------------------

        // A unit quaternion is a quaternion of norm one. Dividing a 
        // non-zero quaternion by its norm produces a unit quaternion
        Quat & normalize();

        // Conjugation of quaternions is analogous to conjugation 
        // of complex numbers.
        Quat & conjugate();

        // Using conjugation and the norm makes it possible to define 
        // the reciprocal of a quaternion. The product of a 
        // quaternion with its reciprocal should equal 1 
        Quat & reciprocal();

        // ----------------------------------------------------------
        // Computes the spherical linear interpolation of two 
        // quaternions. Quaternion Slerps are commonly used to 
        // construct smooth animation curves
        // ----------------------------------------------------------
        
        Quat slerp(const Quat & left, const Quat & right, Real u);
        
        // ----------------------------------------------------------
        // Applies the rotation represented by a quaternion to
        // the specificied vector
        // ----------------------------------------------------------
        
        Vec3 rotate(const Vec3 & v) const;
    };

    // --------------------------------------------------------------
    // Overloaded operators
    // --------------------------------------------------------------

    // Addition, subtraction and product of quaternions. Unlike 
    // multiplication of Real or complex numbers, multiplication of 
    // quaternions is not commutative.
    Quat operator+(const Quat & q1, const Quat & q2);
    Quat operator-(const Quat & q1, const Quat & q2);
    Quat &operator*=(Quat &lhs, const Quat &rhs);
    Quat operator*(const Quat &q1, const Quat &q2);
    
    // Scalar product and division
    Quat &operator*=(Quat & lhs, Real s);
    Quat &operator/=(Quat & lhs,Real s);
    Quat operator*(const Quat &lhs, Real s);
    Quat operator*(Vec3 const & lhs, const Quat &rhs);
    Quat operator/(const Quat &lhs, Real s);
    Quat operator*(Real s, const Quat &val);
    Quat operator*(const Quat &val, Real s);
    Quat operator/(const Quat &val, Real s);

    // Product with a vector (rotates the vector)
    Quat operator*(const Quat &lhs, const Vec3 & rhs);

    // Unary minus (negation)
    Quat operator-(const Quat & q);

    // --------------------------------------------------------------
    // Implementation of inline functions
    // --------------------------------------------------------------
    
    inline Quat::Quat() 
    {
    } 
    
    inline Quat::Quat(Eye) 
    : w(1), x(0), y(0), z(0)
    {
    } 

    inline Quat::Quat(Zero) 
    : w(0), x(0), y(0), z(0)
    {
    }

    inline Quat::Quat(Real w, Real x, Real y, Real z) 
    : x(x), y(y), z(z), w(w) 
    {
    }
    
    inline Quat::Quat(Real w, const Vec3 & xyz) 
    : x(xyz.x), y(xyz.y), z(xyz.z), w(w) 
    {
    }

    inline Vec3 Quat::get_vec_part() const 
    { 
        return Vec3(x, y, z ); 
    }
        
    inline void Quat::set_vec_part(const Vec3 & v) 
    { 
        x = v.x; 
        y = v.y; 
        z = v.z; 
    }

    inline Real * Quat::to_array(Real dest[4]) const
    {
        dest[0] = x;
        dest[1] = y;
        dest[2] = z;
        dest[3] = w;
        return dest;
    }

    inline bool Quat::equals(Quat const & q, Real tolerance) const
    {	
        const Quat aux(w - q.w, x - q.x, y - q.y, z - q.z);
        return aux.dot(aux) < tolerance;
    }

    inline Real Quat::norm2() const
    {
        return dot(*this);
    }

    inline Real Quat::norm() const
    {
        return std::sqrt(norm2());
    }

    inline Quat & Quat::normalize()
    {
        double norm_ = norm();
        if(norm_ < 1e-8)
        {
            throw std::exception();
        }
        *this /= norm_;
        return *this;
    }

    inline Quat & Quat::reciprocal()
    {
        *this = Quat(*this).conjugate() / norm2();
        return *this;
    }

    inline Real Quat::dot(const Quat& rhs) const
    {
        return w * rhs.w + x * rhs.x + y * rhs.y + z * rhs.z;
    }

    inline Quat & Quat::conjugate()
    {
        *this = Quat(w, -x, -y, -z);
        return *this;
    }

    inline Vec3 Quat::rotate(Vec3 const & v) const
    {
        Quat recipr = Quat(*this).reciprocal();
        return ((*this) * Quat(0, v) * recipr).get_vec_part();
    }

    inline Quat operator-(const Quat & q)
    {
        return Quat(-q.w, -q.x, -q.y, -q.z);
    }

    inline Quat &operator*=(Quat &lhs, Real s)
    {
        lhs.w *= s;
        lhs.x *= s;
        lhs.y *= s;
        lhs.z *= s;
        return lhs;
    }

    inline Quat &operator*=(Quat &left, const Quat &right)
    {
        Quat temp;
        temp.w = left.w*right.w - left.x*right.x - 
                 left.y*right.y - left.z*right.z;
        temp.x = left.w*right.x + left.x*right.w + 
                 left.y*right.z - left.z*right.y;
        temp.y = left.w*right.y + left.y*right.w + 
                 left.z*right.x - left.x*right.z;
        temp.z = left.w*right.z + left.z*right.w + 
                 left.x*right.y - left.y*right.x;
        left = temp;
        return left;
    }

    inline Quat &operator/=(Quat &lhs,Real s)
    {
        return lhs *= (1 / s);    
    }
   
    inline Quat operator*(const Quat &lhs,Real s)
    {
        return Quat(lhs.w * s, lhs.x * s, lhs.y * s, lhs.z * s);
    }

    inline Quat operator*(Real s, const Quat &rhs)
    {
        return rhs * s;
    }

    inline Quat operator*(const Quat &lhs, Vec3 const & rhs) 
    {
        return Quat(
            - lhs.x*rhs.x - lhs.y*rhs.y - lhs.z*rhs.z,
              lhs.w*rhs.x + lhs.y*rhs.z - lhs.z*rhs.y,
              lhs.w*rhs.y + lhs.z*rhs.x - lhs.x*rhs.z,
              lhs.w*rhs.z + lhs.x*rhs.y - lhs.y*rhs.x);
    }
    
    inline Quat operator*(Vec3 const & lhs, const Quat &rhs)
    {
        return Quat(
            lhs.x*rhs.x - lhs.y*rhs.y - lhs.z*rhs.z,
            lhs.x*rhs.w + lhs.y*rhs.z - lhs.z*rhs.y,
            lhs.y*rhs.w + lhs.z*rhs.x - lhs.x*rhs.z,
            lhs.z*rhs.w + lhs.x*rhs.y - lhs.y*rhs.x);
    }

    inline Quat operator/(const Quat &lhs,Real s)
    {
        return lhs * (1 / s);
    }

    inline Quat operator*(const Quat &q1, const Quat &q2)
    {
        Quat result = q1;
        result *= q2; 
        return result;
    }

    inline Quat operator+(const Quat & q1, const Quat & q2)
    {
        return Quat(q1.w + q2.w, q1.x + q2.x, 
                    q1.y + q2.y, q1.z + q2.z);
    }

    inline Quat operator-(const Quat & q1, const Quat & q2)
    {
        return q1 + -q2;
    }
}


